#include "dsm2.h"
#include <stdio.h>

const static uint8_t dsm2_header[2] = {0x03, 0x01};
uint8_t dsm_buf[14] = {0};
uint8_t dsm_buf_index = 0;

typedef enum {
	DSM_WAITING_HEADER1 = 0,
	DSM_WAITING_HEADER2,
	DSM_GATHER_DATA
} dsm_parse_status_t;

dsm_parse_status_t dsm_parse_status = DSM_WAITING_HEADER1;

uint8_t dsm_parse_char(uint8_t rx, uint16_t* channels)
{
	uint8_t frame_complete = 0;
	uint8_t i;

	switch (dsm_parse_status) {
		case DSM_WAITING_HEADER1:
			if (rx == dsm2_header[0])
				dsm_parse_status = DSM_WAITING_HEADER2;
			break;
		case DSM_WAITING_HEADER2:
			if (rx == dsm2_header[1]) {
				dsm_buf_index = 0;
				dsm_parse_status = DSM_GATHER_DATA;
				break;
			}
			dsm_parse_status = DSM_WAITING_HEADER1;
			break;
		case DSM_GATHER_DATA:
			dsm_buf[dsm_buf_index++] = rx;
			if (dsm_buf_index < 14)
				break;
			if (*(uint16_t *)&dsm_buf[12] != 0xffff) {
				dsm_parse_status = DSM_WAITING_HEADER1;
				break;
			}
			for (i = 0; i < DSM_CHANNEL_NUM; i++) {
				channels[i] = ((dsm_buf[i<<1] & 0x0003) << 8) | dsm_buf[i<<1+1];   
			}
			dsm_buf_index = DSM_WAITING_HEADER1;
			frame_complete = 1;
			break;
		default:
			break;
	}
	return frame_complete;
}

void dsm_channel_reorder(uint16_t* channels)
{
	//TODO
}
